#include <stdio.h>
#include "485_slave.h"
#include "string.h"
#include "esp_log.h"
#include "math.h"
#include "freertos/FreeRTOS.h"
#include "bsp_main.h"

slave485_t slave485;

esp_err_t slave485_init(slave485_t *slave485, uint8_t address, uint32_t baudrate)
{
    if (slave485 == NULL)
    {
        return ESP_ERR_INVALID_ARG;
    }

    memset(slave485, 0, sizeof(slave485_t));
    slave485->address = address;
    slave485->baudrate = baudrate;
    return ESP_OK;
}

esp_err_t slave485_deinit(slave485_t *slave485)
{
    return ESP_OK;
}

esp_err_t slave485_receive(slave485_t *slave485, uint8_t *data, uint16_t size)
{
    if (data[0] == DEV_ADR)
    {
        if (data[1] == 0x03)
        {
            // if ((uint16_t)data[2] << 8 | (uint16_t)data[3] == 0x00FF)
            // {

            // }
            uint16_t addr = (data[2] << 8) | data[3];
            uint16_t quantity = (data[4] << 8) | data[5];
            uint16_t *buf = malloc(sizeof(uint16_t) * quantity + 5);
            uint8_t *buf_b = (uint8_t *)buf;
            buf_b[0] = DEV_ADR;
            buf_b[1] = 0x03;
            buf_b[2] = (quantity * 2) & 0xFF;
            slave485_read_regs(slave485, addr, (uint16_t *)(buf_b + 3), quantity);
            // change byte order
            uint8_t *cbx = buf_b + 3;
            for (int i = 0; i < quantity; i++)
            {
                uint8_t temp = cbx[i * 2];
                cbx[i * 2] = cbx[i * 2 + 1];
                cbx[i * 2 + 1] = temp;
            }
            // CRC
            uint16_t crc = crc16(buf_b, 3 + 2 * quantity, 0x8005);
            buf_b[3 + 2 * quantity] = (uint8_t)(crc & 0xFF);
            buf_b[4 + 2 * quantity] = (uint8_t)(crc >> 8);
            bsp_uart_a_tx((uint8_t *)buf, quantity * 2 + 5);
            free(buf);
        }
        else if (data[1] == 0x06)
        {
            uint16_t addr = (data[2] << 8) | data[3];
            uint16_t value = (data[4] << 8) | data[5];
            slave485_write_reg(slave485, addr, &value);
            // CRC
            bsp_uart_a_tx(data, 8);
        }
        else if (data[1] == 0x01)
        {
            uint16_t addr = (data[2] << 8) | data[3];
            uint16_t quantity = (data[4] << 8) | data[5];
            uint16_t *buf = malloc(sizeof(uint8_t) * quantity + 5);
            buf[0] = DEV_ADR;
            buf[1] = 0x01;
            buf[2] = quantity & 0xFF;
            slave485_read_coil(slave485, addr, buf + 3, quantity);
            // CRC
            uint16_t crc = crc16(buf, 3 + quantity, 0x8005);
            buf[3 + quantity] = crc & 0xFF;
            buf[4 + quantity] = crc >> 8;
            bsp_uart_a_tx(buf, quantity + 5);
            free(buf);
        }
        else if (data[1] == 0x05)
        {
            uint16_t addr = (data[2] << 8) | data[3];
            uint16_t value = (data[4] << 8) | data[5];
            slave485_write_coil(slave485, addr, &value);
            // CRC
            bsp_uart_a_tx(data, 8);
        }
    }
    printf("\n");
    return ESP_OK;
}

esp_err_t slave485_add_reg(slave485_t *slave485, slave_reg_t *reg)
{
    if (slave485 == NULL || reg == NULL)
    {
        ESP_LOGW("slave485", "Invalid argument");
        return ESP_ERR_INVALID_ARG;
    }

    slave_reg_t *tmp = &slave485->reg;
    while (tmp->next != NULL)
    {
        if (tmp->next->adr == reg->adr)
        {
            ESP_LOGE("slave485", "Register already exists: %d", reg->adr);
            return ESP_ERR_INVALID_ARG;
        }
        tmp = tmp->next;
    }
    tmp->next = malloc(sizeof(slave_reg_t));
    if (tmp->next == NULL)
    {
        ESP_LOGW("slave485", "Memory allocation failed");
        return ESP_ERR_NO_MEM;
    }

    memset(tmp->next, 0, sizeof(slave_reg_t));
    tmp->next->adr = reg->adr;
    tmp->next->data = malloc(reg->size * sizeof(uint16_t));
    if (tmp->next->data == NULL)
    {
        ESP_LOGW("slave485", "Memory allocation failed");
        free(tmp->next);
        tmp->next = NULL;
        return ESP_ERR_NO_MEM;
    }

    memset(tmp->next->data, 0x00, reg->size * sizeof(uint16_t));

    tmp->next->size = reg->size;
    tmp->next->updated = false;
    tmp->next->cb = reg->cb;

    return ESP_OK;
}

esp_err_t slave485_add_coil(slave485_t *slave485, slave_coil_t *coil)
{
    if (slave485 == NULL || coil == NULL)
    {
        ESP_LOGW("slave485", "Invalid argument");
        return ESP_ERR_INVALID_ARG;
    }

    slave_coil_t *tmp = &slave485->coil;
    while (tmp->next != NULL)
    {
        if (tmp->next->adr == coil->adr)
        {
            ESP_LOGE("slave485", "Coil already exists: %d", coil->adr);
            return ESP_ERR_INVALID_ARG;
        }
        tmp = tmp->next;
    }
    tmp->next = malloc(sizeof(slave_coil_t));
    if (tmp->next == NULL)
    {
        ESP_LOGW("slave485", "Memory allocation failed");
        return ESP_ERR_NO_MEM;
    }

    memset(tmp->next, 0, sizeof(slave_coil_t));
    tmp->next->adr = coil->adr;
    tmp->next->data = malloc(coil->size * sizeof(uint16_t));
    if (tmp->next->data == NULL)
    {
        ESP_LOGW("slave485", "Memory allocation failed");
        free(tmp->next);
        tmp->next = NULL;
        return ESP_ERR_NO_MEM;
    }

    memset(tmp->next->data, 0x00, coil->size * sizeof(uint16_t));

    tmp->next->size = coil->size;
    tmp->next->updated = false;
    tmp->next->cb = coil->cb;

    return ESP_OK;
}

esp_err_t slave485_remove_reg(slave485_t *slave485, uint16_t address)
{
    if (slave485 == NULL)
    {
        ESP_LOGW("slave485", "Invalid argument");
        return ESP_ERR_INVALID_ARG;
    }

    slave_reg_t *tmp = &slave485->reg;
    while (tmp->next != NULL)
    {
        if (tmp->next->adr == address)
        {
            slave_reg_t *tmp2 = tmp->next->next;
            free(tmp->next->data);
            free(tmp->next);
            tmp->next = tmp2;
            return ESP_OK;
        }
        tmp = tmp->next;
    }

    return ESP_OK;
}

esp_err_t slave485_read_regs(slave485_t *slave485, uint16_t address, uint16_t *data, uint16_t size)
{
    if (slave485 == NULL)
    {
        ESP_LOGW("slave485", "Invalid argument");
        return ESP_ERR_INVALID_ARG;
    }

    slave_reg_t *tmp = &slave485->reg;
    bool found = false;
    while (tmp->next != NULL)
    {
        for (int i = 0; i < tmp->next->size; i++)
        {
            if (tmp->next->adr + i == address)
            {
                if (size > tmp->next->size)
                {
                    ESP_LOGW("slave485", "Invalid argument, size: %d", size);
                    return ESP_ERR_INVALID_ARG;
                }
                memcpy(data, &tmp->next->data[i], size * sizeof(uint16_t));
                found = true;
                return ESP_OK;
            }
        }
        tmp = tmp->next;
    }

    return found ? ESP_OK : ESP_ERR_NOT_FOUND;
}

esp_err_t slave485_write_reg(slave485_t *slave485, uint16_t address, uint16_t data)
{
    if (slave485 == NULL)
    {
        ESP_LOGW("slave485", "Invalid argument");
        return ESP_ERR_INVALID_ARG;
    }

    slave_reg_t *tmp = &slave485->reg;
    bool found = false;
    while (tmp->next != NULL)
    {
        for (int i = 0; i < tmp->next->size; i++)
        {
            if (tmp->next->adr + i == address)
            {
                memcpy(&tmp->next->data[i], data, sizeof(uint16_t));
                found = true;
                // tmp->next->updated = true;
                if (tmp->next->cb != NULL)
                {
                    tmp->next->cb(tmp->next->data, i);
                }
                return ESP_OK;
            }
        }
        tmp = tmp->next;
    }

    return found ? ESP_OK : ESP_ERR_NOT_FOUND;
}

esp_err_t slave485_read_coil(slave485_t *slave485, uint16_t adr, uint8_t *data, uint16_t size)
{
    if (slave485 == NULL)
    {
        ESP_LOGW("slave485", "Invalid argument");
        return ESP_ERR_INVALID_ARG;
    }
    slave_coil_t *temp = &slave485->coil;
    int data_len;
    while (temp->next != NULL)
    {
        for (int i = 0; i < temp->next->size; i++)
            if (temp->next->adr + i == adr)
            {
                if (size > temp->next->size)
                    return ESP_ERR_INVALID_ARG;
                if (size % 8 == 0)
                    data_len = size / 8;
                else
                    data_len = size / 8 + 1;
                // data = malloc(data_len);
                for (int j = 0; j < data_len; j++)
                {
                    data[j] = 0x00;
                    int byte_effective_len = fmin(8, size - 8 * j);
                    for (int k = 0; k < byte_effective_len; k++)
                        data[j] |= temp->next->data[j * 8 + k] == 0xFF00 ? (0x01 << k) : 0x00;
                }
                return ESP_OK;
            }
        temp = temp->next;
    }
    return ESP_ERR_NOT_FOUND;
}

esp_err_t slave485_write_coil(slave485_t *slave485, uint16_t adr, uint8_t *data)
{
    if (slave485 == NULL)
    {
        ESP_LOGW("slave485", "Invalid argument");
        return ESP_ERR_INVALID_ARG;
    }
    slave_coil_t *temp = &slave485->coil;
    while (temp->next != NULL)
    {
        for (int i = 0; i < temp->next->size; i++)
            if (temp->next->adr + i == adr)
            {
                memcpy(&temp->next->data[i], data, sizeof(uint16_t));
                // temp->next->data[1] = data;
                if (temp->next->cb != NULL)
                {
                    temp->next->cb(temp->next->data, i);
                }
                return ESP_OK;
            }
        temp = temp->next;
    }
    return ESP_ERR_NOT_FOUND;
}

void print_485_regs()
{
    slave_reg_t *tmp;
    tmp = slave485.reg.next;
    while (tmp != NULL)
    {
        printf("%04X:[ ", tmp->adr);
        for (int i = 0; i < tmp->size; i++)
        {
            printf("%04X ", tmp->data[i]);
        }
        printf("]\n");
        tmp = tmp->next;
    }
}
